package ar.edu.untref.robotica.motorfisico;

import java.awt.Point;

import ar.edu.untref.robotica.simulador.Coordenadas;
import ar.edu.untref.robotica.simulador.Escenario;

/**
 * Pide y recibe informaci�n del entorno y los obstaculos en el mismo desde la
 * clase escenario.
 * 
 * @author Untref
 *
 */
public class Sensor {

	// Angulo en el que apunta, se toma con respecto al frente del robot (al
	// frente es 0 grados, atras 180)
	private double anguloRespectoAFrente = 0;
	private double posicionX; // posiciones de sensor con respecto al centro del
	private double posicionY; // robot.

	public Sensor(double angulo, double posicionX, double posicionY) {
		this.anguloRespectoAFrente = angulo;
		this.posicionX = posicionX;
		this.posicionY = posicionY;
	}

	public double sensar(Escenario escenario) {

		double anguloDeSensado = escenario.getPosicionRobot().getAngulo()
				+ anguloRespectoAFrente;
		double epsilon = 1; //0.0001 si no muy lento (ver)

		double hipo = Math.sqrt(Math.pow(posicionX, 2)+ Math.pow(posicionY, 2));
		double anguloRelativo = Math.atan(posicionX/posicionY)*-1;
		double anguloAbsoluto = Math.toRadians(escenario.getPosicionRobot().getAngulo()) + anguloRelativo;

//		System.out.println("hipotenusa: "+hipo);
//		System.out.println("angulo relativo: "+Math.toDegrees(anguloRelativo));
//		System.out.println("angulo obsoluto: "+Math.toDegrees(anguloAbsoluto));

		double posicionActualSensorY;
		double posicionActualSensorX;
		
		//cambia si esta debajo del eje Y
		if(posicionY < 0){
			posicionActualSensorY = escenario.getPosicionRobot().getPosicionY() - hipo* Math.cos(anguloAbsoluto);
			posicionActualSensorX = escenario.getPosicionRobot().getPosicionX() + hipo* Math.sin(anguloAbsoluto);
		}else{
			posicionActualSensorY = escenario.getPosicionRobot().getPosicionY() + hipo* Math.cos(anguloAbsoluto);
			posicionActualSensorX = escenario.getPosicionRobot().getPosicionX() - hipo* Math.sin(anguloAbsoluto);
		}
		
//		System.out.println("pos robot X: "+escenario.getPosicionRobot().getPosicionX());
//		System.out.println("pos robot y: "+escenario.getPosicionRobot().getPosicionY());
////		System.out.println("correccion sensor X: "+hipo* Math.sin(anguloAbsoluto));
////		System.out.println("correccion sensor Y: "+hipo* Math.cos(anguloAbsoluto));
//		System.out.println("pos sensor X: "+posicionActualSensorX);
//		System.out.println("pos sensor Y: "+posicionActualSensorY);
		
		Coordenadas coordenadasObstaculo = new Coordenadas(posicionActualSensorX, posicionActualSensorY);

		// mientras no sea un obstaculo
		do{
			coordenadasObstaculo.setPosicionX(coordenadasObstaculo.getPosicionX()
					+ epsilon * Math.sin(Math.toRadians(anguloDeSensado)));
			coordenadasObstaculo.setPosicionY(coordenadasObstaculo.getPosicionY()
					- epsilon * Math.cos(Math.toRadians(anguloDeSensado)));
		}while(escenario.getMatriz()[(int) coordenadasObstaculo.getPosicionX()][(int) coordenadasObstaculo
		        .getPosicionY()] != 1); 
		
		return Point.distance(posicionActualSensorX,posicionActualSensorY,
				coordenadasObstaculo.getPosicionX(),
				coordenadasObstaculo.getPosicionY());
	}

}
